/*
 * main.c
 */
#include "System\F2806x_Device.h"
#include <stdio.h>
#include <string.h>
#include "Drivers\debugLED.h"
#include "Timer.h"
#include "CANDriver.h"
#include "Datafile.h"
#include "Drivers\BMP085Drv.h"
#include "Drivers\LSM303DLLinear.h"
#include "Drivers\LSM303DLMag.h"
#include "Drivers\UARTDriverDBG.h"
#include "Drivers\UARTDriverB.h"
#include "Drivers\GPSModule.h"
#include "Drivers\HSCMRPressure.h"
#include "Drivers\GyroL3G4200.h"
#include "Processor.h"

extern "C" void InitSysCtrl(void);

// flash support
#define FLASH 1
extern "C" void InitFlash(void);
extern "C" void MemCopy(Uint16 *SourceAddr, Uint16* SourceEndAddr, Uint16* DestAddr);
extern Uint16 RamfuncsLoadStart, RamfuncsLoadEnd, RamfuncsRunStart;


#define CAN_BASE 0x030000
// DUAL IMU! XXX
//#define CAN_BASE 0x035000

// Drivers
DebugLED ledDebug;

// Components
Timer timer;
Timer timerINT;
CANDriver canDrv;
SDataFile df;
BMP085Drv bmpDrv;
LSM303DLLinear accDrv;
LSM303DLMag magDrv;
UARTDriverDBG uartDBG;
GPSModule gpsDrv;
HSCMRPressure hscmPressure;
GyroL3G4200 gyroDrv;
CProcessor processor;

// main step time
#define STEPTIME 0.01 /* standard steptime, 100 Hz */

float offCalib = 0;

int main(void)
{
	//=================================
	//	INITIALISATION - General
	//=================================
	InitSysCtrl();	// Device Life support & GPIO mux settings

	// init debug LED
	ledDebug.Init();

	// init Serial
	uartDBG.Init(57600);
	uartDBG.SendData("IMUBoard v3.1 - Q10 edition IMU\n\r");

// Only used if running from FLASH
#ifdef FLASH
// Copy time critical code and Flash setup code to RAM
// The  RamfuncsLoadStart, RamfuncsLoadEnd, and RamfuncsRunStart
// symbols are created by the linker. Refer to the linker files.
	MemCopy(&RamfuncsLoadStart, &RamfuncsLoadEnd, &RamfuncsRunStart);

// Call Flash Initialization to setup flash waitstates
// This function must reside in RAM
	InitFlash();	// Call the flash wrapper init function
#endif //(FLASH)

	// Init Components
	timer.Init();
	timerINT.Init();
	canDrv.Init();
	bmpDrv.Init();
	magDrv.Init();
	accDrv.Init();
	hscmPressure.Init();
	gpsDrv.Initialize();
	gyroDrv.Init();
	processor.Reset();

	// Reset DataFile
	memset(&df, 0, sizeof(df));

	// Enable global Interrupts and higher priority real-time debug events:
	EINT;   // Enable Global interrupt INTM
	ERTM;	// Enable Global realtime interrupt DBGM

//=================================
//	Endless LOOP
//=================================
	for(;;)  //infinite loop
	{
		timer.BeginPeriod();
		df.Counter++;

		bmpDrv.Process(); // test BMP085
		accDrv.Process();
		magDrv.Process();
		gpsDrv.Update(); // process GPS
		hscmPressure.Process();
		gyroDrv.Update();

		// get GPS data
		df.gpsSatCount = gpsDrv.GetSatUsed();
		df.gpsLat = gpsDrv.GetLatitude();
		df.gpsLon = gpsDrv.GetLongitude();
		df.gpsAlt = gpsDrv.GetAltitude();
		df.gpsVelX = gpsDrv.GetVelX();
		df.gpsVelY = gpsDrv.GetVelY();
		df.gpsReceived = gpsDrv.GetReceivedAutoreset();

		// other
		df.AbsPressure = bmpDrv.GetPressure();
		df.DiffPressure = hscmPressure.GetPressure();
		df.Temperature = hscmPressure.GetTemperature();

		// convert data
		df.gyroRollX = processor.DegToRadian(gyroDrv.m_gyroX); // to [rad/s]
		df.gyroPitchY = processor.DegToRadian(gyroDrv.m_gyroY); // to [rad/s]
		df.gyroYawZ = processor.DegToRadian(gyroDrv.m_gyroZ); // to [rad/s]

		// CORRECTION/OFFSETs
		//offCalib = offCalib*0.99 + 0.01 * df.gyroRollX;
		// Normal board
		df.gyroRollX -= 0.0160f;
		df.gyroPitchY -= 0.0025f;
		df.gyroYawZ += 0.0020f;
		// Bugged board
		//df.gyroRollX -= 0.336f;
		//df.gyroPitchY += 0.295f;
		//df.gyroYawZ -= 0.274f;

		df.accX = accDrv.m_AxisY*9.81f; // to [m/s^2]
		df.accY = accDrv.m_AxisX*9.81f; // to [m/s^2]
		df.accZ = -accDrv.m_AxisZ*9.81f; // to [m/s^2]
		df.magX = magDrv.m_AxisY;
		df.magY = magDrv.m_AxisX;
		df.magZ = -magDrv.m_AxisZ;
		// XXX - Add Mag. offsets! (move from xbeeboard)

		// calculate RPY
		processor.AddNewMeasurement(df.gyroRollX, df.gyroPitchY, df.gyroYawZ, df.accX, df.accY, df.accZ, STEPTIME);
		processor.GetRollPitchYaw(df.Roll, df.Pitch, df.Yaw); // [rad]


		// dump all data to CAN
		// 100Hz
		canDrv.SendMBOX(0, df.gyroRollX);
		canDrv.SendMBOX(1, -df.gyroPitchY); // fix inverse rotation
		canDrv.SendMBOX(2, -df.gyroYawZ); // fix inverse rotation
		canDrv.SendMBOX(3, df.Roll);
		canDrv.SendMBOX(4, df.Pitch);
		canDrv.SendMBOX(5, df.Yaw);
		canDrv.SendMBOX(6, df.AbsPressure);
		canDrv.SendMBOX(7, df.DiffPressure);
		canDrv.SendMBOX(25, df.accX);
		canDrv.SendMBOX(26, df.accY);
		canDrv.SendMBOX(27, df.accZ);
		canDrv.SendMBOX(13, df.magX); // XXX
		canDrv.SendMBOX(14, df.magY);
		canDrv.SendMBOX(15, df.magZ);

		// 10 Hz
		if( (df.Counter % 10) == 0)
		{
			canDrv.SendMBOX(8, df.Temperature);

			canDrv.SendMBOX(20, df.LoopTimeMS);
			canDrv.SendMBOX(21, df.MissionTime);
			canDrv.SendMBOX(22, df.LoopTimeMSMAX);
		}

		// GPS: send only when received!
		if(df.gpsReceived)
		{
			canDrv.SendMBOX(9, df.gpsLon);
			canDrv.SendMBOX(10, df.gpsLat);
			canDrv.SendMBOX(11, df.gpsAlt);
			canDrv.SendMBOX(12, (float)df.gpsSatCount);
			canDrv.SendMBOX(16, df.gpsVelX);
			canDrv.SendMBOX(17, df.gpsVelY);
		}

		// GPS LOOPBACK!!!
		//char buffer[250];
		//int len = gpsDrv.GetData(buffer);
		//uartDBG.QueueData(buffer, len);
		//len = uartDBG.GetData(buffer);
		//gpsDrv.QueueData(buffer, len);

		// blink LED
		if( gpsDrv.GetSatUsed() == 0)
		{
			if( (df.Counter % 50) == 0) ledDebug.Toggle(DebugLED::GREEN);
		}
		else
		{
			if( (df.Counter % 5) == 0) ledDebug.Toggle(DebugLED::GREEN);
		}

		//-----------------------------------------------------------
		// finish loop
		df.LoopTimeMS = timer.ElapsedTime() * 1000; // get looptime (ms)
		if(df.LoopTimeMSMAX < df.LoopTimeMS) df.LoopTimeMSMAX = df.LoopTimeMS;

		while( !timer.HasElapsed(STEPTIME) )
		{
			// do something...
		};
		df.MissionTime += timer.ElapsedTime();
	}
}

// SETUP BOXES
void CANSetupBoxes(CANDriver* ptr)
{
	// TX
	ptr->SetupMBOX(0,  CAN_BASE+0x000300, 8, true); // GyroX
	ptr->SetupMBOX(1,  CAN_BASE+0x000301, 8, true); // GyroY
	ptr->SetupMBOX(2,  CAN_BASE+0x000302, 8, true); // GyroZ
	ptr->SetupMBOX(25,  CAN_BASE+0x000303, 8, true); // AccX
	ptr->SetupMBOX(26,  CAN_BASE+0x000304, 8, true); // AccY
	ptr->SetupMBOX(27,  CAN_BASE+0x000305, 8, true); // AccZ
	ptr->SetupMBOX(13,  CAN_BASE+0x000306, 8, true); // MagX
	ptr->SetupMBOX(14,  CAN_BASE+0x000307, 8, true); // MagY
	ptr->SetupMBOX(15,  CAN_BASE+0x000308, 8, true); // MagZ

	ptr->SetupMBOX(3,  CAN_BASE+0x000310, 8, true); // Roll
	ptr->SetupMBOX(4,  CAN_BASE+0x000311, 8, true); // Pitch
	ptr->SetupMBOX(5,  CAN_BASE+0x000312, 8, true); // Yaw
	ptr->SetupMBOX(6,  CAN_BASE+0x000320, 8, true); // Abs. Pressure
	ptr->SetupMBOX(7,  CAN_BASE+0x000321, 8, true); // Diff. Pressure
	ptr->SetupMBOX(8,  CAN_BASE+0x000322, 8, true); // Temperature

	ptr->SetupMBOX(9,  CAN_BASE+0x000330, 8, true); // GPS Long
	ptr->SetupMBOX(10,  CAN_BASE+0x000331, 8, true); // GPS Lat
	ptr->SetupMBOX(11,  CAN_BASE+0x000332, 8, true); // GPS Alt
	ptr->SetupMBOX(12,  CAN_BASE+0x000333, 8, true); // GPS Sat#

	ptr->SetupMBOX(16,  CAN_BASE+0x000335, 8, true); // GPS Vel-X
	ptr->SetupMBOX(17,  CAN_BASE+0x000336, 8, true); // GPS Vel-Y

	ptr->SetupMBOX(20,  CAN_BASE+0x000350, 8, true); // Loop time
	ptr->SetupMBOX(21,  CAN_BASE+0x000351, 8, true); // TotalTime
	ptr->SetupMBOX(22,  CAN_BASE+0x000352, 8, true); // LooptimeMAX
}
